#include <stdlib.h>

#include "lift_info.h"
#include "board_config.h"
#include "system_sm.h"
#include "board_relay.h"

struct lift_ctrl_handle lift = {0};

void lift_ctrl_reset_total_floor(uint8_t floor)
{
    HAL_GPIO_WritePin(GRID1_GPIO_Port, GRID1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GRID2_GPIO_Port, GRID2_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GRID3_GPIO_Port, GRID3_Pin, GPIO_PIN_RESET);

    lift.oc_flag.value = 0;

    lift.max_height_pos = 0;
    lift.sul_pos = 0;

    lift.total_floor = floor;

    lift.is_floor_info_learned = false;

    lift.is_cover_enable = false;

    lift.specified_speed = 40;
    board_config_write(CONFIG_SPECIFIED_SPEED, 40);

    board_config_write(CONFIG_LEARNING_OF_FLOOR_HEIGHT, 0);

    lift.up_slow_down_pos = 250;
    board_config_write(CONFIG_TOP_FORCE_POS, 250);
    lift.down_slow_down_pos = 250;
    board_config_write(CONFIG_BUTTOM_FORCE_POS, 250);

    lift.up_flat_pos = 30;
    board_config_write(CONFIG_TOP_FLAT_LAYER, 30);
    lift.down_flat_pos = 30;
    board_config_write(CONFIG_BUTTOM_FLAT_LAYER, 30);

    for (int i = 0; i < MAX_TOTAL_FLOOR; i++) {
        lift.floor_info[i].flat_pos = 0;
        lift.floor_info[i].up_slow_down_pos = 0;
        lift.floor_info[i].down_slow_down_pos = 0;
        lift.floor_info[i].up_flat_pos = 0;
        lift.floor_info[i].down_flat_pos = 0;
        lift.floor_info[i].up_flat_trimming_pos = DEFAULT_TOP_TRIMMING_POS;
        lift.floor_info[i].down_flat_trimming_pos = DEFAULT_DOWN_TRIMMING_POS;
        
        board_config_floor_info_write(CONFIG_FLAT_POS_ADDR, i, 0);
        board_config_floor_info_write(CONFIG_UP_SLOW_DOWN_POS_ADDR, i, 0);
        board_config_floor_info_write(CONFIG_DOWN_SLOW_DOWN_POS_ADDR, i, 0);
        board_config_floor_info_write(CONFIG_UP_TRIMMING_POS_ADDR, i, DEFAULT_TOP_TRIMMING_POS);
        board_config_floor_info_write(CONFIG_DOWN_TRIMMING_POS_ADDR, i, DEFAULT_DOWN_TRIMMING_POS);
    }
}

void lift_ctrl_set_flat_pos(uint8_t floor, uint32_t pos)
{
    if (floor + 1 > lift.total_floor) {
        return;
    }

    // 有平层位置，不允许修改
    if (lift.floor_info[floor].flat_pos && lift.is_cover_enable == false) {
        return;
    }

    if (lift.floor_info[floor].flat_pos == 0) {
        lift.floor_info[floor].up_slow_down_pos = pos + lift.up_slow_down_pos;
        lift.floor_info[floor].down_slow_down_pos = pos - lift.down_slow_down_pos;

        board_config_floor_info_write(CONFIG_UP_SLOW_DOWN_POS_ADDR, floor, lift.floor_info[floor].up_slow_down_pos);
        board_config_floor_info_write(CONFIG_DOWN_SLOW_DOWN_POS_ADDR, floor, lift.floor_info[floor].down_slow_down_pos);
    }

    lift.floor_info[floor].flat_pos = pos;

    lift.floor_info[floor].up_flat_pos = pos + lift.up_flat_pos;
    lift.floor_info[floor].up_flat_pos += ((int)lift.floor_info[floor].up_flat_trimming_pos - 50);

    lift.floor_info[floor].down_flat_pos = pos - lift.down_flat_pos;
    lift.floor_info[floor].down_flat_pos += ((int)lift.floor_info[floor].down_flat_trimming_pos - 50);

    if (floor == lift.total_floor - 1) {
        lift.sul_pos = pos + DEFUALT_SUL_HEIGHT_THAN_TOP;
        lift.max_height_pos = pos + DEFUALT_MAX_HEIGHT_THAN_TOP;
    }

    lift.is_floor_info_learned = lift_is_floor_info_learned();

    board_config_floor_info_write(CONFIG_FLAT_POS_ADDR, floor, pos);
}

void lift_ctrl_set_up_flat_trimming_pos(uint8_t floor, uint16_t trimming_pos)
{
    if (floor + 1 > lift.total_floor) {
        return;
    }

    lift.floor_info[floor].up_flat_trimming_pos = trimming_pos;

    lift.floor_info[floor].up_flat_pos = lift.floor_info[floor].flat_pos + lift.up_flat_pos;
    lift.floor_info[floor].up_flat_pos += ((int)lift.floor_info[floor].up_flat_trimming_pos - 50);

    board_config_floor_info_write(CONFIG_UP_TRIMMING_POS_ADDR, floor, trimming_pos);
}

void lift_ctrl_set_down_flat_trimming_pos(uint8_t floor, uint16_t trimming_pos)
{
    if (floor + 1 > lift.total_floor) {
        return;
    }

    lift.floor_info[floor].down_flat_trimming_pos = trimming_pos;

    lift.floor_info[floor].down_flat_pos = lift.floor_info[floor].flat_pos - lift.down_flat_pos;
    lift.floor_info[floor].down_flat_pos += ((int)lift.floor_info[floor].down_flat_trimming_pos - 50);

    board_config_floor_info_write(CONFIG_DOWN_TRIMMING_POS_ADDR, floor, trimming_pos);
}



enum lift_state {
    LIFT_STATE_STOP = 0,
    LIFT_STATE_UP,
    LIFT_STATE_DOWN,
};


struct lift_info {
    bool is_data_valid;
    uint16_t invalid_cnt;       // 不合法计数
    uint32_t position;
    uint8_t floor;   
    uint32_t disconnect_tick;
    bool slow_down_flag;
    bool is_flat_floor;
    enum lift_state state;
};

static volatile struct lift_info lift_info = {0};

bool lift_is_floor_info_learned(void)
{
    if (lift.total_floor == 0) {
        return false;
    }

    for (int i = 0; i < lift.total_floor; i++) {
        if (lift.floor_info[i].flat_pos == 0) {
            return false;
        }
    }

    return true;
}

void lift_disconnect_check(void)
{
    if (lift_info.disconnect_tick < LIFT_CAN_MAX_DISCONNECT_MS) {
        lift_info.disconnect_tick++;
    } else {
        lift.oc_flag.oc_flag.sensor_disconn = 1;

        if (lift.is_floor_info_learned) {
            board_relay_oc_ctrl(GPIO_PIN_RESET);
        }
    }
}

static enum lift_state lift_get_state_form_speed(int16_t speed)
{
    static uint8_t lift_up_cnt = 0;
    static uint8_t lift_down_cnt = 0;
    static uint8_t lift_stop_cnt = 0;
    
    if (speed > LIFT_MIN_SPEED) {
        lift_down_cnt = 0;
        lift_stop_cnt = 0;
        if (lift_up_cnt < LIFT_STATE_CNT) {
            lift_up_cnt++;
        } else {
            return LIFT_STATE_UP;
        }
    } else if (speed < -LIFT_MIN_SPEED) {
        lift_up_cnt = 0;
        lift_stop_cnt = 0;
        if (lift_down_cnt < LIFT_STATE_CNT) {
            lift_down_cnt++;
        } else {
            return LIFT_STATE_DOWN;
        }
    } else {
        lift_up_cnt = 0;
        lift_down_cnt = 0;
        if (lift_stop_cnt < LIFT_STATE_CNT) {
            lift_stop_cnt++;
        } else {
            return LIFT_STATE_STOP;
        }
    }

    return lift_info.state;
}

static void lift_get_current_floor(uint32_t position, uint8_t total_floor)
{
    if (position <= lift.floor_info[0].up_slow_down_pos) {
        lift.current_floor = 0;
    } else if (position >= lift.floor_info[total_floor - 1].down_slow_down_pos) {
        lift.current_floor = total_floor - 1;
    } else {
        for (uint16_t i = 0; i < total_floor; i++) {
            if (position <= lift.floor_info[i].up_slow_down_pos && position >= lift.floor_info[i].down_slow_down_pos) {
                lift.current_floor = i;
                break;
            } else if (position < lift.floor_info[i + 1].down_slow_down_pos) {
                break;
            }
        }
    }
}

static __attribute__((unused)) void lift_slow_down_ctrl_by_state(uint32_t position, uint8_t total_floor)
{
    enum lift_section {
        LIFT_INIT_SECTION,
        LIFT_SLOW_DOWN_SECTION_UP,
        LIFT_SLOW_DOWN_SECTION_DOWN,
        LIFT_RUNNING_SECTION,
    };

    static enum lift_section section = LIFT_INIT_SECTION;
    
    if (section == LIFT_INIT_SECTION) {
        if (lift_info.state == LIFT_STATE_UP) {
            for (uint16_t i = 0; i < total_floor; i++) {
                if (position >= lift.floor_info[i].down_slow_down_pos && position <= lift.floor_info[i].down_slow_down_pos + LIFT_SLOW_DOWN_ZONE) {
                    section = LIFT_SLOW_DOWN_SECTION_UP;
                    board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
                    return;
                }
            }
            board_relay_slow_down_close();
            section = LIFT_RUNNING_SECTION;
            return;
        } else if (lift_info.state == LIFT_STATE_DOWN) {
            for (uint16_t i = 0; i < total_floor; i++) {
                if (position <= lift.floor_info[i].up_slow_down_pos && position >= lift.floor_info[i].up_slow_down_pos - LIFT_SLOW_DOWN_ZONE) {
                    section = LIFT_SLOW_DOWN_SECTION_DOWN;
                    board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
                    return;
                }
            }
            board_relay_slow_down_close();
            section = LIFT_RUNNING_SECTION;
            return;
        } else {
            section = LIFT_INIT_SECTION;
            return;
        }
    } else if (section == LIFT_SLOW_DOWN_SECTION_UP) {
        for (uint16_t i = 0; i < total_floor; i++) {
            if (position >= lift.floor_info[i].down_slow_down_pos && position <= lift.floor_info[i].down_slow_down_pos + LIFT_SLOW_DOWN_ZONE) {
                section = LIFT_SLOW_DOWN_SECTION_UP;
                board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
                return;
            }
        }
        board_relay_slow_down_close();
        section = LIFT_RUNNING_SECTION;
        return;
    } else if (section == LIFT_SLOW_DOWN_SECTION_DOWN) {
        for (uint16_t i = 0; i < total_floor; i++) {
            if (position <= lift.floor_info[i].up_slow_down_pos && position >= lift.floor_info[i].up_slow_down_pos - LIFT_SLOW_DOWN_ZONE) {
                section = LIFT_SLOW_DOWN_SECTION_DOWN;
                board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
                return;
            }
        }
        board_relay_slow_down_close();
        section = LIFT_RUNNING_SECTION;
        return;
    } else if (section == LIFT_RUNNING_SECTION) {
        if (lift_info.state == LIFT_STATE_UP) {
            for (uint16_t i = 0; i < total_floor; i++) {
                if (position >= lift.floor_info[i].down_slow_down_pos && position <= lift.floor_info[i].down_slow_down_pos + LIFT_SLOW_DOWN_ZONE) {
                    section = LIFT_SLOW_DOWN_SECTION_UP;
                    board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
                    return;
                }
            }
            board_relay_slow_down_close();
            section = LIFT_RUNNING_SECTION;
            return;
        } else if (lift_info.state == LIFT_STATE_DOWN) {
            for (uint16_t i = 0; i < total_floor; i++) {
                if (position <= lift.floor_info[i].up_slow_down_pos && position >= lift.floor_info[i].up_slow_down_pos - LIFT_SLOW_DOWN_ZONE) {
                    section = LIFT_SLOW_DOWN_SECTION_DOWN;
                    board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
                    return;
                }
            }
            board_relay_slow_down_close();
            section = LIFT_RUNNING_SECTION;
            return;
        } else {
            board_relay_slow_down_close();
            section = LIFT_RUNNING_SECTION;
            return;
        }
    }
}

static void lift_slow_down_ctrl(uint32_t position, uint8_t total_floor)
{
    for (uint16_t i = 0; i < total_floor; i++) {
        if (position >= lift.floor_info[i].down_slow_down_pos && position <= lift.floor_info[i].down_slow_down_pos + LIFT_SLOW_DOWN_ZONE) {
            board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
            return;
        }
        if (position <= lift.floor_info[i].up_slow_down_pos && position >= lift.floor_info[i].up_slow_down_pos - LIFT_SLOW_DOWN_ZONE) {
            board_relay_slow_down_ctrl(i, GPIO_PIN_SET);
            return;
        }
    }

    board_relay_slow_down_close();
}

static void lift_flat_floor_ctrl(uint32_t position, uint16_t total_floor)
{
    for (uint16_t i = 0; i < total_floor; i++) {
        if (position <= lift.floor_info[i].up_flat_pos && position >= lift.floor_info[i].down_flat_pos) {
            board_relay_flat_floor_ctrl(i, GPIO_PIN_SET);
            return;
        }
    }

    board_relay_flat_floor_close();
    return;
}

static void board_oc_ctrl(uint32_t position, int16_t speed)
{
    // 速度需要先 × 10才是真实速度（mm/s）
    if (abs(speed) >= 11.5 * lift.specified_speed) {
        lift.oc_flag.oc_flag.overspeed = 1;
        board_relay_oc_ctrl(GPIO_PIN_RESET);
    } else {
        lift.oc_flag.oc_flag.overspeed = 0;
    }

    if (position >= lift.max_height_pos) {
        lift.oc_flag.oc_flag.overheight = 1;
        board_relay_oc_ctrl(GPIO_PIN_RESET);
    } else {
        lift.oc_flag.oc_flag.overheight = 0;
    }

    if (lift.oc_flag.value == 0) {
        board_relay_oc_ctrl(GPIO_PIN_SET);
    }
}

static void board_relay_active(uint32_t position, int16_t speed)
{
    board_oc_ctrl(position, speed);

    // 超过顶层5公分
    if (position >= lift.sul_pos) {
        board_relay_sul_ctrl(GPIO_PIN_SET);
    } else {
        board_relay_sul_ctrl(GPIO_PIN_RESET);
    }
    
    lift_info.state = lift_get_state_form_speed(speed);

    lift_get_current_floor(position, lift.total_floor);
    lift_flat_floor_ctrl(position, lift.total_floor);
    lift_slow_down_ctrl(position, lift.total_floor);
}

void lift_info_analysis(uint8_t *data)
{
    uint8_t status = data[6];
    uint32_t position = *(uint32_t*)data;
    int16_t speed = *(int16_t*)&data[4];

    lift_info.disconnect_tick = 0;
    lift.oc_flag.oc_flag.sensor_disconn = 0;

    if (is_hardware_testing) {
        return;
    }

        // 数据合法
    if (status == 0x7) {
        lift_info.position = position;
        lift_info.is_data_valid = true;
    }

    if (lift.is_floor_info_learned == false) {
        return;
    }

    // 数据合法
    if (status == 0x7) {
        lift_info.invalid_cnt = 0;
        lift.oc_flag.oc_flag.can_comm_err = 0;
        board_relay_active(position, speed);
    } else {
        lift_info.is_data_valid = false;
        if (lift_info.invalid_cnt < LIFT_SENSOR_MAX_ERR_CNT) {
            lift_info.invalid_cnt++;
        } else {
            lift.oc_flag.oc_flag.can_comm_err = 1;
            board_relay_oc_ctrl(GPIO_PIN_RESET);
        }
    }
}

int lift_info_get_position(uint32_t *position)
{
    if (lift_info.is_data_valid) {
        *position = lift_info.position;
        return 0;
    }
    
    return -1;
}









